"""

This Circuit Python code to control the Robot Quadruped "PICO Python Dog" can be freely used and modified, providing:

This code must NOT be used for commercial use or for profit.

This comment must remain in the code at the top of the .py file and this comment must NOT be modified or removed.

"""

import time

import board

import busio

import pwmio

from adafruit_motor import servo

import adafruit_icm20x

from math import asin, acos, atan, degrees, sqrt

import pulseio

import adafruit_irremote

# IR sensor

IR_PIN = board.GP9 # Pin GP9 connected to IR receiver

# IR decoder

pulsein = pulseio.PulseIn(IR_PIN, maxlen=100, idle_state=True)

decoder = adafruit_irremote.GenericDecode()

pulsein.clear()

pulsein.resume()

# Acc Gyro Mag test setup on i2c (1)

i2c = busio.I2C(board.GP11, board.GP10)

icm = adafruit_icm20x.ICM20948(i2c)

# AGM functions

def Acc(): # Acc +/- 10 for 90 degrees of x and y axis (wobble board +...)

Ax = round(icm.acceleration[0]) # Gyro detects sudden movement in real time (compliance +...)

Ay = round(icm.acceleration[1]) # Mag (compass) for up side down detection

return Ax, Ay

# Servo setup (Raspberry Pi PICO micro controller (Circuit Python) with Tower Pro MG90S servos

FL_knee = servo.Servo(pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FL_hip = servo.Servo(pwmio.PWMOut(board.GP1, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_knee = servo.Servo(pwmio.PWMOut(board.GP27, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_hip = servo.Servo(pwmio.PWMOut(board.GP26, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_knee = servo.Servo(pwmio.PWMOut(board.GP14, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_hip = servo.Servo(pwmio.PWMOut(board.GP15, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_knee = servo.Servo(pwmio.PWMOut(board.GP21, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_hip = servo.Servo(pwmio.PWMOut(board.GP20, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

# servo function - converts Cartesian inputs to degrees of angles of servos

def leg_control(FLx,FLy,FRx,FRy,BLx,BLy,BRx,BRy):

FRcal = -5 # calibration factor

FLhip, FLknee = rec2pol(FLx, FLy) # This IK code is interpolated by default!

FRhip, FRknee = rec2pol(FRx, FRy)

BLhip, BLknee = rec2pol(BLx, BLy)

BRhip, BRknee = rec2pol(BRx, BRy)

# degrees do NOT have to be integers

FL_knee.angle = 180 - FLknee

FL_hip.angle = 180 - FLhip

FR_knee.angle = FRknee + FRcal

FR_hip.angle = FRhip

BL_knee.angle = 180 - BLknee

BL_hip.angle = 180 - BLhip

BR_knee.angle = BRknee

BR_hip.angle = BRhip

def rec2pol(x,y): # upper and lower leg are both 50mm

hypotenuse = sqrt(y**2+x**2)

hip = 90+(degrees((atan(x/y))-acos(((hypotenuse**2)/(100*hypotenuse)))))

knee = degrees(acos(1-((hypotenuse**2)/5000)))

return hip, knee

def numbers(start, finish): # list of numbers from 'start' to 'finish' for smooth linear leg movement

numbers = []

if start < finish:

for new_number in range(start, finish, 1):

for fraction in range(0, 10, 2):

value = new_number + fraction/10

numbers.append(value)

else:

for new_number in range(start-1, finish-1, -1):

for fraction in range(10, 0, -2):

value = new_number + fraction/10

numbers.append(value)

numbers.append(finish+0.0)

return numbers

""" MAIN CODE """

time.sleep(3)

# EXERCISES

# STARTS AT LAY POSITION

# leg_control(FLx, FLy, FRx, FRy, BLx, BLy, BRx, BRy)

delay = 0.002

x = numbers(20, 80) # from lay position to walk position

for c in range(0, len(x)):

leg_control(0, x[c], 0, x[c], 0, x[c], 0, x[c])

time.sleep(delay)

time.sleep(1)

x = numbers(80, 10) # from walk position to lay position

for c in range(0, len(x)):

leg_control(0, x[c], 0, x[c], 0, x[c], 0, x[c])

time.sleep(delay)

time.sleep(1)

x = numbers(10, 70) # from lay to walk position

for c in range(0, len(x)):

leg_control(0, x[c], 0, x[c], 0, x[c], 0, x[c])

time.sleep(delay)

time.sleep(1)

x = numbers(0, -20) # from walk to slew forward

for c in range(0, len(x)):

leg_control(x[c], 70, x[c], 70, x[c], 70, x[c], 70) # wrt foot under hip

time.sleep(delay)

time.sleep(1)

x = numbers(-20, 10) # from slew forward to slew back

for c in range(0, len(x)):

leg_control(x[c], 70, x[c], 70, x[c], 70, x[c], 70) # -ve to +ve wrt foot under hip

time.sleep(delay)

time.sleep(1)

x = numbers(10, 0) # from slew forward to walk position

for c in range(0, len(x)):

leg_control(x[c], 70, x[c], 70, x[c], 70, x[c], 70) # and not the direction of the body

time.sleep(delay)

time.sleep(1)

x = numbers(70, 60) # from walk to lower walk

for c in range(0, len(x)):

leg_control(0, x[c], 0, x[c], 0, x[c], 0, x[c])

time.sleep(delay)

time.sleep(1)

x = numbers(60, 51) # from walk to roll left

for c in range(0, len(x)):

leg_control(0, x[c], 0, 60, 0, x[c], 0, 60)

time.sleep(delay*2.5)

time.sleep(1)

x = numbers(51, 60) # from roll left to roll right

for c in range(0, len(x)):

leg_control(0, x[c], 0, 60, 0, x[c], 0, 60)

time.sleep(delay*2.5)

x = numbers(60, 51) # from walk to roll left

for c in range(0, len(x)):

leg_control(0, 60, 0, x[c], 0, 60, 0, x[c])

time.sleep(delay*2.5)

time.sleep(1)

x = numbers(51, 60) # from roll left to walk position

for c in range(0, len(x)):

leg_control(0, 60, 0, x[c], 0, 60, 0, x[c])

time.sleep(delay*2.5)

time.sleep(1)

x = numbers(60, 20) # from walk position to lay position

for c in range(0, len(x)):

leg_control(0, x[c], 0, x[c], 0, x[c], 0, x[c])

time.sleep(delay)

time.sleep(1)

time.sleep(3)

# CRAWL

# leg_control(FLx, FLy, FRx, FRy, BLx, BLy, BRx, BRy)

delay = 0.002

while True:

leg_control(-40, 60, 40, 60, 40, 60, -40, 60)

time.sleep(delay)

leg_control(-39, 59.4, 39, 60, 39, 60, -39, 59.4)

time.sleep(delay)

leg_control(-38, 58.6, 38, 60, 38, 60, -38, 58.6)

time.sleep(delay)

leg_control(-37, 57.8, 37, 60, 37, 60, -37, 57.8)

time.sleep(delay)

leg_control(-36, 57, 36, 60, 36, 60, -36, 57)

time.sleep(delay)

leg_control(-35, 56.3, 35, 60, 35, 60, -35, 56.3)

time.sleep(delay)

leg_control(-34, 55.6, 34, 60, 34, 60, -34, 55.6)

time.sleep(delay)

leg_control(-33, 54.9, 33, 60, 33, 60, -33, 54.9)

time.sleep(delay)

leg_control(-32, 54.2, 32, 60, 32, 60, -32, 54.2)

time.sleep(delay)

leg_control(-31, 53.6, 31, 60, 31, 60, -31, 53.6)

time.sleep(delay)

leg_control(-30, 53, 30, 60, 30, 60, -30, 53)

time.sleep(delay)

leg_control(-29, 52.5, 29, 60, 29, 60, -29, 52.5)

time.sleep(delay)

leg_control(-28, 51.9, 28, 60, 28, 60, -28, 51.9)

time.sleep(delay)

leg_control(-27, 51.4, 27, 60, 27, 60, -27, 51.4)

time.sleep(delay)

leg_control(-26, 50.9, 26, 60, 26, 60, -26, 50.9)

time.sleep(delay)

leg_control(-25, 50.5, 25, 60, 25, 60, -25, 50.5)

time.sleep(delay)

leg_control(-24, 50, 24, 60, 24, 60, -24, 50)

time.sleep(delay)

leg_control(-23, 49.6, 23, 60, 23, 60, -23, 49.6)

time.sleep(delay)

leg_control(-22, 49.2, 22, 60, 22, 60, -22, 49.2)

time.sleep(delay)

leg_control(-21, 48.8, 21, 60, 21, 60, -21, 48.8)

time.sleep(delay)

leg_control(-20, 48.4, 20, 60, 20, 60, -20, 48.4)

time.sleep(delay)

leg_control(-19, 48.1, 19, 60, 19, 60, -19, 48.1)

time.sleep(delay)

leg_control(-18, 47.8, 18, 60, 18, 60, -18, 47.8)

time.sleep(delay)

leg_control(-17, 47.5, 17, 60, 17, 60, -17, 47.5)

time.sleep(delay)

leg_control(-16, 47.2, 16, 60, 16, 60, -16, 47.2)

time.sleep(delay)

leg_control(-15, 46.9, 15, 60, 15, 60, -15, 46.9)

time.sleep(delay)

leg_control(-14, 46.7, 14, 60, 14, 60, -14, 46.7)

time.sleep(delay)

leg_control(-13, 46.4, 13, 60, 13, 60, -13, 46.4)

time.sleep(delay)

leg_control(-12, 46.2, 12, 60, 12, 60, -12, 46.2)

time.sleep(delay)

leg_control(-11, 46, 11, 60, 11, 60, -11, 46)

time.sleep(delay)

leg_control(-10, 45.8, 10, 60, 10, 60, -10, 45.8)

time.sleep(delay)

leg_control(-9, 45.7, 9, 60, 9, 60, -9, 45.7)

time.sleep(delay)

leg_control(-8, 45.5, 8, 60, 8, 60, -8, 45.5)

time.sleep(delay)

leg_control(-7, 45.4, 7, 60, 7, 60, -7, 45.4)

time.sleep(delay)

leg_control(-6, 45.3, 6, 60, 6, 60, -6, 45.3)

time.sleep(delay)

leg_control(-5, 45.2, 5, 60, 5, 60, -5, 45.2)

time.sleep(delay)

leg_control(-4, 45.1, 4, 60, 4, 60, -4, 45.1)

time.sleep(delay)

leg_control(-3, 45.1, 3, 60, 3, 60, -3, 45.1)

time.sleep(delay)

leg_control(-2, 45, 2, 60, 2, 60, -2, 45)

time.sleep(delay)

leg_control(-1, 45, 1, 60, 1, 60, -1, 45)

time.sleep(delay)

leg_control(0, 45, 0, 60, 0, 60, 0, 45)

time.sleep(delay)

leg_control(1, 45, -1, 60, -1, 60, 1, 45)

time.sleep(delay)

leg_control(2, 45, -2, 60, -2, 60, 2, 45)

time.sleep(delay)

leg_control(3, 45.1, -3, 60, -3, 60, 3, 45.1)

time.sleep(delay)

leg_control(4, 45.1, -4, 60, -4, 60, 4, 45.1)

time.sleep(delay)

leg_control(5, 45.2, -5, 60, -5, 60, 5, 45.2)

time.sleep(delay)

leg_control(6, 45.3, -6, 60, -6, 60, 6, 45.3)

time.sleep(delay)

leg_control(7, 45.4, -7, 60, -7, 60, 7, 45.4)

time.sleep(delay)

leg_control(8, 45.5, -8, 60, -8, 60, 8, 45.5)

time.sleep(delay)

leg_control(9, 45.7, -9, 60, -9, 60, 9, 45.7)

time.sleep(delay)

leg_control(10, 45.8, -10, 60, -10, 60, 10, 45.8)

time.sleep(delay)

leg_control(11, 46, -11, 60, -11, 60, 11, 46)

time.sleep(delay)

leg_control(12, 46.2, -12, 60, -12, 60, 12, 46.2)

time.sleep(delay)

leg_control(13, 46.4, -13, 60, -13, 60, 13, 46.4)

time.sleep(delay)

leg_control(14, 46.7, -14, 60, -14, 60, 14, 46.7)

time.sleep(delay)

leg_control(15, 46.9, -15, 60, -15, 60, 15, 46.9)

time.sleep(delay)

leg_control(16, 47.2, -16, 60, -16, 60, 16, 47.2)

time.sleep(delay)

leg_control(17, 47.5, -17, 60, -17, 60, 17, 47.5)

time.sleep(delay)

leg_control(18, 47.8, -18, 60, -18, 60, 18, 47.8)

time.sleep(delay)

leg_control(19, 48.1, -19, 60, -19, 60, 19, 48.1)

time.sleep(delay)

leg_control(20, 48.4, -20, 60, -20, 60, 20, 48.4)

time.sleep(delay)

leg_control(21, 48.8, -21, 60, -21, 60, 21, 48.8)

time.sleep(delay)

leg_control(22, 49.2, -22, 60, -22, 60, 22, 49.2)

time.sleep(delay)

leg_control(23, 49.6, -23, 60, -23, 60, 23, 49.6)

time.sleep(delay)

leg_control(24, 50, -24, 60, -24, 60, 24, 50)

time.sleep(delay)

leg_control(25, 50.5, -25, 60, -25, 60, 25, 50.5)

time.sleep(delay)

leg_control(26, 50.9, -26, 60, -26, 60, 26, 50.9)

time.sleep(delay)

leg_control(27, 51.4, -27, 60, -27, 60, 27, 51.4)

time.sleep(delay)

leg_control(28, 51.9, -28, 60, -28, 60, 28, 51.9)

time.sleep(delay)

leg_control(29, 52.5, -29, 60, -29, 60, 29, 52.5)

time.sleep(delay)

leg_control(30, 53, -30, 60, -30, 60, 30, 53)

time.sleep(delay)

leg_control(31, 53.6, -31, 60, -31, 60, 31, 53.6)

time.sleep(delay)

leg_control(32, 54.2, -32, 60, -32, 60, 32, 54.2)

time.sleep(delay)

leg_control(33, 54.9, -33, 60, -33, 60, 33, 54.9)

time.sleep(delay)

leg_control(34, 55.6, -34, 60, -34, 60, 34, 55.6)

time.sleep(delay)

leg_control(35, 56.3, -35, 60, -35, 60, 35, 56.3)

time.sleep(delay)

leg_control(36, 57, -36, 60, -36, 60, 36, 57)

time.sleep(delay)

leg_control(37, 57.8, -37, 60, -37, 60, 37, 57.8)

time.sleep(delay)

leg_control(38, 58.6, -38, 60, -38, 60, 38, 58.6)

time.sleep(delay)

leg_control(39, 59.4, -39, 60, -39, 60, 39, 59.4)

time.sleep(delay)

leg_control(40, 60, -40, 60, -40, 60, 40, 60)

time.sleep(delay)

# half way point

leg_control(40, 60, -40, 60, -40, 60, 40, 60)

time.sleep(delay)

leg_control(39, 60, -39, 59.4, -39, 59.4, 39, 60)

time.sleep(delay)

leg_control(38, 60, -38, 58.6, -38, 58.6, 38, 60)

time.sleep(delay)

leg_control(37, 60, -37, 57.8, -37, 57.8, 37, 60)

time.sleep(delay)

leg_control(36, 60, -36, 57, -36, 57, 36, 60)

time.sleep(delay)

leg_control(35, 60, -35, 56.3, -35, 56.3, 35, 60)

time.sleep(delay)

leg_control(34, 60, -34, 55.6, -34, 55.6, 34, 60)

time.sleep(delay)

leg_control(33, 60, -33, 54.9, -33, 54.9, 33, 60)

time.sleep(delay)

leg_control(32, 60, -32, 54.2, -32, 54.2, 32, 60)

time.sleep(delay)

leg_control(31, 60, -31, 53.6, -31, 53.6, 31, 60)

time.sleep(delay)

leg_control(30, 60, -30, 53, -30, 53, 30, 60)

time.sleep(delay)

leg_control(29, 60, -29, 52.5, -29, 52.5, 29, 60)

time.sleep(delay)

leg_control(28, 60, -28, 51.9, -28, 51.9, 28, 60)

time.sleep(delay)

leg_control(27, 60, -27, 51.4, -27, 51.4, 27, 60)

time.sleep(delay)

leg_control(26, 60, -26, 50.9, -26, 50.9, 26, 60)

time.sleep(delay)

leg_control(25, 60, -25, 50.5, -25, 50.5, 25, 60)

time.sleep(delay)

leg_control(24, 60, -24, 50, -24, 50, 24, 60)

time.sleep(delay)

leg_control(23, 60, -23, 49.6, -23, 49.6, 23, 60)

time.sleep(delay)

leg_control(22, 60, -22, 49.2, -22, 49.2, 22, 60)

time.sleep(delay)

leg_control(21, 60, -21, 48.8, -21, 48.8, 21, 60)

time.sleep(delay)

leg_control(20, 60, -20, 48.4, -20, 48.4, 20, 60)

time.sleep(delay)

leg_control(19, 60, -19, 48.1, -19, 48.1, 19, 60)

time.sleep(delay)

leg_control(18, 60, -18, 47.8, -18, 47.8, 18, 60)

time.sleep(delay)

leg_control(17, 60, -17, 47.5, -17, 47.5, 17, 60)

time.sleep(delay)

leg_control(16, 60, -16, 47.2, -16, 47.2, 16, 60)

time.sleep(delay)

leg_control(15, 60, -15, 46.9, -15, 46.9, 15, 60)

time.sleep(delay)

leg_control(14, 60, -14, 46.7, -14, 46.7, 14, 60)

time.sleep(delay)

leg_control(13, 60, -13, 46.4, -13, 46.4, 13, 60)

time.sleep(delay)

leg_control(12, 60, -12, 46.2, -12, 46.2, 12, 60)

time.sleep(delay)

leg_control(11, 60, -11, 46, -11, 46, 11, 60)

time.sleep(delay)

leg_control(10, 60, -10, 45.8, -10, 45.8, 10, 60)

time.sleep(delay)

leg_control(9, 60, -9, 45.7, -9, 45.7, 9, 60)

time.sleep(delay)

leg_control(8, 60, -8, 45.5, -8, 45.5, 8, 60)

time.sleep(delay)

leg_control(7, 60, -7, 45.4, -7, 45.4, 7, 60)

time.sleep(delay)

leg_control(6, 60, -6, 45.3, -6, 45.3, 6, 60)

time.sleep(delay)

leg_control(5, 60, -5, 45.2, -5, 45.2, 5, 60)

time.sleep(delay)

leg_control(4, 60, -4, 45.1, -4, 45.1, 4, 60)

time.sleep(delay)

leg_control(3, 60, -3, 45.1, -3, 45.1, 3, 60)

time.sleep(delay)

leg_control(2, 60, -2, 45, -2, 45, 2, 60)

time.sleep(delay)

leg_control(1, 60, -1, 45, -1, 45, 1, 60)

time.sleep(delay)

leg_control(0, 60, 0, 45, 0, 45, 0, 60)

time.sleep(delay)

leg_control(-1, 60, 1, 45, 1, 45, -1, 60)

time.sleep(delay)

leg_control(-2, 60, 2, 45, 2, 45, -2, 60)

time.sleep(delay)

leg_control(-3, 60, 3, 45.1, 3, 45.1, -3, 60)

time.sleep(delay)

leg_control(-4, 60, 4, 45.1, 4, 45.1, -4, 60)

time.sleep(delay)

leg_control(-5, 60, 5, 45.2, 5, 45.2, -5, 60)

time.sleep(delay)

leg_control(-6, 60, 6, 45.3, 6, 45.3, -6, 60)

time.sleep(delay)

leg_control(-7, 60, 7, 45.4, 7, 45.4, -7, 60)

time.sleep(delay)

leg_control(-8, 60, 8, 45.5, 8, 45.5, -8, 60)

time.sleep(delay)

leg_control(-9, 60, 9, 45.7, 9, 45.7, -9, 60)

time.sleep(delay)

leg_control(-10, 60, 10, 45.8, 10, 45.8, -10, 60)

time.sleep(delay)

leg_control(-11, 60, 11, 46, 11, 46, -11, 60)

time.sleep(delay)

leg_control(-12, 60, 12, 46.2, 12, 46.2, -12, 60)

time.sleep(delay)

leg_control(-13, 60, 13, 46.4, 13, 46.4, -13, 60)

time.sleep(delay)

leg_control(-14, 60, 14, 46.7, 14, 46.7, -14, 60)

time.sleep(delay)

leg_control(-15, 60, 15, 46.9, 15, 46.9, -15, 60)

time.sleep(delay)

leg_control(-16, 60, 16, 47.2, 16, 47.2, -16, 60)

time.sleep(delay)

leg_control(-17, 60, 17, 47.5, 17, 47.5, -17, 60)

time.sleep(delay)

leg_control(-18, 60, 18, 47.8, 18, 47.8, -18, 60)

time.sleep(delay)

leg_control(-19, 60, 19, 48.1, 19, 48.1, -19, 60)

time.sleep(delay)

leg_control(-20, 60, 20, 48.4, 20, 48.4, -20, 60)

time.sleep(delay)

leg_control(-21, 60, 21, 48.8, 21, 48.8, -21, 60)

time.sleep(delay)

leg_control(-22, 60, 22, 49.2, 22, 49.2, -22, 60)

time.sleep(delay)

leg_control(-23, 60, 23, 49.6, 23, 49.6, -23, 60)

time.sleep(delay)

leg_control(-24, 60, 24, 50, 24, 50, -24, 60)

time.sleep(delay)

leg_control(-25, 60, 25, 50.5, 25, 50.5, -25, 60)

time.sleep(delay)

leg_control(-26, 60, 26, 50.9, 26, 50.9, -26, 60)

time.sleep(delay)

leg_control(-27, 60, 27, 51.4, 27, 51.4, -27, 60)

time.sleep(delay)

leg_control(-28, 60, 28, 51.9, 28, 51.9, -28, 60)

time.sleep(delay)

leg_control(-29, 60, 29, 52.5, 29, 52.5, -29, 60)

time.sleep(delay)

leg_control(-30, 60, 30, 53, 30, 53, -30, 60)

time.sleep(delay)

leg_control(-31, 60, 31, 53.6, 31, 53.6, -31, 60)

time.sleep(delay)

leg_control(-32, 60, 32, 54.2, 32, 54.2, -32, 60)

time.sleep(delay)

leg_control(-33, 60, 33, 54.9, 33, 54.9, -33, 60)

time.sleep(delay)

leg_control(-34, 60, 34, 55.6, 34, 55.6, -34, 60)

time.sleep(delay)

leg_control(-35, 60, 35, 56.3, 35, 56.3, -35, 60)

time.sleep(delay)

leg_control(-36, 60, 36, 57, 36, 57, -36, 60)

time.sleep(delay)

leg_control(-37, 60, 37, 57.8, 37, 57.8, -37, 60)

time.sleep(delay)

leg_control(-38, 60, 38, 58.6, 38, 58.6, -38, 60)

time.sleep(delay)

leg_control(-39, 60, 39, 59.4, 39, 59.4, -39, 60)

time.sleep(delay)

leg_control(-40, 60, 40, 60, 40, 60, -40, 60)

time.sleep(delay)